#include <iostream>
#include <vector>
#include <cmath>

using namespace std;

#include "cgal_helper.h"
#include "DiamondTest.h"
#include "Face.h"
#include "Triangulation.h"
#include "ConvexHull.h"
#include "Prim_MST.h"
#include "ConvexHull.h"
#include "kmeans/KMlocal.h"
#include "GreedyTriangulation.h"

Triangulation::Triangulation(vector<Point> p_list): p_list(p_list) {}

vector<Face *> Triangulation::diamond_test() {
  vector <Face *> f_list;

  for (unsigned int i = 0; i < p_list.size(); i++) {
		fprintf(stderr, "%d\n", i);
    for (unsigned int j = i+1; j < p_list.size(); j++) {
      for (unsigned int k = j+1; k < p_list.size(); k++) {
        DiamondTest test(p_list [i], p_list [j], p_list [k]);
        int p_inside  = -1;
        bool valid    = true;
        for (unsigned int l = 0; l < p_list.size(); l++) {
          if (l == i || l == j || l == k)
            continue;
          if (!test.testPoint(p_list [l]))
            continue;
          if (CGAL::ON_ORIENTED_BOUNDARY == test.getOrientation(p_list [l])) {
            valid = false;
            break;
          }

          if (p_inside == -1) {
            p_inside  = l;
          } else {
            if (test.getOrientation(p_list [l]) == test.getOrientation(p_list [p_inside])) {
              continue;
            } else {
              valid = false;
              break;
            }
          }
        }

        if (valid) {
          f_list.push_back(new Face(i,j,k));
					cout << p_list[i] << " " << p_list[j] << " " << p_list[k] << endl;
				}
      }
    }
  }
  return f_list;
}

FT Triangulation::weight(CGALTriangulation &t) {
  FT wt = 0.0;
  for (CGALTriangulation::Finite_edges_iterator it = t.finite_edges_begin(); it != t.finite_edges_end(); it++) {
    wt  += sqrt(t.segment(*it).squared_length());
  }
  return wt;
}

FT Triangulation::area(CGALTriangulation &t) {
  FT wt = 0.0;

  for (CGALTriangulation::Finite_facets_iterator it = t.finite_facets_begin(); it != t.finite_facets_end(); it++) {
    wt  += sqrt(t.triangle(*it).squared_area());
  }
  return wt;
}

FT Triangulation::average_distance() {
  static FT avg_dist  = 0.0;
  if (avg_dist != 0.0)
    return avg_dist/((p_list.size()*(p_list.size()-1))/2);
  for (unsigned int i = 0; i < p_list.size(); i++) {
    for (unsigned int j = i+1; j < p_list.size(); j++) {
      avg_dist  += sqrt(Segment(p_list [i], p_list [j]).squared_length());
    }
  }
  return avg_dist/((p_list.size()*(p_list.size()-1))/2);
}

int Triangulation::degree(CGALTriangulation &t, CGALTriangulation::Cell_handle &cell, int s, int e) {
  int degree  = 0;
  CGALTriangulation::Cell_circulator ccir = t.incident_cells(cell, s, e);
  CGALTriangulation::Cell_circulator cend = ccir;
  do {
    degree++;
    ++ccir;
  } while (ccir != cend);
  return degree;
}

int Triangulation::degree(CGALTriangulation &t, const CGALTriangulation::Edge &edge) {
  int degree  = 0;
  CGALTriangulation::Cell_circulator ccir = t.incident_cells(edge);
  CGALTriangulation::Cell_circulator cend = ccir;
  do {
    degree++;
    ++ccir;
  } while (ccir != cend);
  return degree;
}

bool Triangulation::angle_flip(CGALTriangulation &t, CGALTriangulation::Facet f) {
  CGALPlane plane(f.first->vertex((f.second+1)&3)->point(), f.first->vertex((f.second+2)&3)->point(), f.first->vertex((f.second+3)&3)->point());
  Point projection  = plane.projection(f.first->vertex((f.second)&3)->point());
  FT h_l  = Segment(f.first->vertex((f.second+1)&3)->point(), f.first->vertex((f.second)&3)->point()).squared_length();
  FT b_l  = Segment(f.first->vertex((f.second+1)&3)->point(), projection).squared_length(); 

  double a_deg  = acos(sqrt(b_l/h_l))*180/PI;

  if (a_deg > 30)
    return true;
  return false;
}

bool Triangulation::face_flip(CGALTriangulation &t, CGALTriangulation::Facet f) {
  CGALTriangulation::Cell_handle n  = f.first->neighbor(f.second);
  int in  = n->index(f.first);

  Triangle t1(f.first->vertex(f.second)->point(), f.first->vertex((f.second+1)&3)->point(), n->vertex(in)->point());
  Triangle t2(f.first->vertex(f.second)->point(), f.first->vertex((f.second+2)&3)->point(), n->vertex(in)->point());
  Triangle t3(f.first->vertex(f.second)->point(), f.first->vertex((f.second+3)&3)->point(), n->vertex(in)->point());
  if ((t1.squared_area() + t2.squared_area() + t3.squared_area()) < t.triangle(f).squared_area())
    return true;
  return false;
}

bool Triangulation::edge_flip(CGALTriangulation &t, CGALTriangulation::Facet f, bool is_area) {
  CGALTriangulation::Cell_handle n  = f.first->neighbor(f.second);
  int in  = n->index(f.first);

  if (degree(t, f.first, (f.second+1)&3, (f.second+3)&3) == 3) {
    if (!is_area)
      return true;
    Triangle t1(f.first->vertex(f.second)->point(), f.first->vertex((f.second+1)&3)->point(), f.first->vertex((f.second+3)&3)->point());
    Triangle t3(n->vertex(in)->point(), f.first->vertex((f.second+1)&3)->point(), f.first->vertex((f.second+3)&3)->point());
    Triangle t_flip(f.first->vertex(f.second)->point(), f.first->vertex((f.second+2)&3)->point(), n->vertex(in)->point());
    if ((t1.squared_area() + t.triangle(f).squared_area() + t3.squared_area()) > t_flip.squared_area())
      return true;
  }
  return false;
}

void Triangulation::alter_for_better(CGALTriangulation &t, bool is_area) {
  bool changed  = true;

  /*
  cout << "Triangulation before optimization" << endl;
  cout << "Edges: " << t.number_of_finite_edges() << " Cells: " << t.number_of_finite_cells() << endl;
  if (is_area) {
    cout << "Area: " << area(t) << endl;
    cout << "Average Area: " << area(t)/t.number_of_finite_facets() << endl;
  } else {
    cout << "Weight: " << weight(t) << endl;
    cout << "Average Weight: " << weight(t)/t.number_of_finite_edges() << endl;
  }*/
  cout << "Weight (Sum of edges in triangulation): " << weight(t) << endl;
  while(changed) {
    changed = false;
    for (CGALTriangulation::Finite_facets_iterator it = t.finite_facets_begin(); it != t.finite_facets_end(); it++) {
      CGALTriangulation::Cell_handle n  = it->first->neighbor(it->second);
      if (!t.is_infinite(n) && edge_flip(t, *it, is_area) && t.flip(it->first, (it->second+1)&3, (it->second+3)&3)) {
        //Flips 3-tetrahedra to 2-tetrahedra.
        changed = true;
        break;
      }
    }
  }

 /* for (CGALTriangulation::Finite_facets_iterator it = t.finite_facets_begin(); it != t.finite_facets_end(); it++) {
    cout << "(" << it->first->vertex((it->second+1)&3)->point().x() << "," <<  it->first->vertex((it->second+1)&3)->point().y() << "," << it->first->vertex((it->second+1)&3)->point().z() << ")";
    cout << "(" << it->first->vertex((it->second+2)&3)->point().x() << "," <<  it->first->vertex((it->second+2)&3)->point().y() << "," << it->first->vertex((it->second+2)&3)->point().z() << ")";
    cout << "(" << it->first->vertex((it->second+3)&3)->point().x() << "," <<  it->first->vertex((it->second+3)&3)->point().y() << "," << it->first->vertex((it->second+3)&3)->point().z() << ")";
    cout << endl;
  }
*/
  cout << "Weight (Sum of edges in triangulation): " << weight(t) << endl;
  /*
  cout << endl;
  cout << "Triangulation after optimization" << endl;
  cout << "Edges: " << t.number_of_finite_edges() << " Cells: " << t.number_of_finite_cells() << endl;
  if (is_area) {
    cout << "Area: " << area(t) << endl;
    cout << "Average Area: " << area(t)/t.number_of_finite_facets() << endl;
  } else {
    cout << "Weight: " << weight(t) << endl;
    cout << "Average Weight: " << weight(t)/t.number_of_finite_edges() << endl;
  }
  cout << "Valid: " << (t.is_valid()? "True": "False")<< endl;
  */
}

template <class T>
void Triangulation::run_triangulation(bool is_area) {
  /**
   * Pre-process points.
  **/
  /*
  KMterm term(
    500, 0, 0, 0, // run for 100 stages
    0.10, 0.10, 3, // other typical parameter values
    0.50, 10, 0.95
    );
  int k = 300;
  KMdata dataPts(3, p_list.size());
  for (unsigned int i = 0; i < p_list.size(); i++) {
    dataPts[i][0]  = (KMcoord) (p_list [i].x());
    dataPts[i][1]  = (KMcoord) (p_list [i].y());
    dataPts[i][2]  = (KMcoord) (p_list [i].z());
  }
  dataPts.buildKcTree();
  KMfilterCenters ctrs(k, dataPts);
  KMlocalLloyds kmAlg(ctrs, term);
  ctrs = kmAlg.execute();

  KMctrIdxArray closeCtr = new KMctrIdx[dataPts.getNPts()];  
  double* sqDist = new double[dataPts.getNPts()];
  vector<Point> c_list;
  ctrs.getAssignments(closeCtr, sqDist);
  
  for (int j = 0; j < k; j++) {
    for (unsigned int i = 0; i < p_list.size(); i++) {
      if (closeCtr [i] == j) {
        c_list.push_back(p_list [i]);
      }
    }
  }
  delete [] closeCtr;
  delete [] sqDist;
  */
  /*GreedyTriangulation g_t(p_list);
  g_t.run();
  alter_for_better(g_t.get_triangulation());*/
  //cout << average_distance() << endl;
  
	CGALTriangulation t = T(p_list.begin(), p_list.end());
  alter_for_better(t, is_area);
}

void Triangulation::run() {
  run_triangulation<CGALDelaunay>();
}
